% QUADPOD Kinematics
% -------------------
% The construction uses 3DOF construction for arms. The tip of each
% arm can be set into any arbitrary position [x,y,z], limited by
% physical construction of the arm only.
%
% A coordinated positioning of all the arm create a gait. This
% simulation shows the logic used to calculate the movement, having
% a fixed gait.  The gait is implemented as 1-3-2-4, where the
% topology of servos is shown in the drawing:
%
%                       +y
%                	1 +----|----+ 2
%                   | Q2 | Q1 |
%            -x -------- CG ------- +x
%                   | Q3 | Q4 |
%                	4 +----|----+ 3
%                	      -y
% 
% CG = Center of Gravity, axis x and y. The angle gamma start in Q1 and
% rotates counterclockwise. Gamma is the angle positioning the shoulder
% pivot.
%
% All the coordinates are calculated for the orthogonal x/y system.
% The orientation of the system is relative to center of gravity of
% the quadpod. All the shoulder servos are then rotated. The angle
% between the plane of servo and plane of quadpod's body is depicted
% in the schematics drawings:
%
% ARM No.1: angle 45 deg. 
% (plane of servo must be rotated 45 degrees )
% (clockwise to meet the x-y plane of the robot)
% ----------------------
%          servo +y   +y
%                \    |    / servo +x
%                  \  |  /         
%           -x ______\|/___0____+x 
%                   / |\   360     
%                 /   |  \         
%               /     |    \       
%                     -y           
%
% ARM No.2: 315 deg
% -------------------
%                     +y
%                \    |    / servo +y
%                  \  |  /
%           -x ______\|/___0____+x
%                   / |\   360
%                 /   |  \
%               /     |    \
%                     -y  servo +x
%
% ARM No.3: 225 deg
% -------------------
%                     +y
%                \    |    /          
%                  \  |  /
%           -x ______\|/___0____+x
%                   / |\   360
%                 /   |  \
%               /     |    \servo +y
%          servo +x   -y
%
% ARM No.4: 135 deg
% -------------------
%          servo +x   +y
%                \    |    /         
%                  \  |  /
%           -x ______\|/___0____+x
%                   / |\   360
%                 /   |  \
%               /     |    \
%          servo +y   -y
%
% Next to this definition we define the sweet point for each arm.
% The sweep point is the SP[x,y,z]. The tip of arm can move within 
% the circle having the centre at sweet point and radius of step/2.
% For the given construction is sweet point:
%
%  [51,51,51] = [72,0,51] = [0,72,51]
%
%   generally for each combination of x,y where:
%                  72=hypot(x,y);
%
% The movement is than given by creating a vector V, where x=0, z=0
% and y in <-step/2;+step/2>. The reference point is SP. This requires
% to transpose SP into location [0,0,0]. Direction of the movement 
% is achieved by rotating the vector around the transposed SP. 
% After rotation, we superpose SP back to it original location at 
% [51,51,51].
%
% Define physical dimensions of the robot.
coxa=14.8;		% Length of COXA
femur=45.45;	% Length of FEMUR
tibia=59.29;	% Length of TIBIA
base=92; 		% Base is the distance of the COXA to the z=0
% Based on this physical dimensions, the maximal step can be 
% achieved as 102mm, preferably 98mm. Minimal step height is 0, 
% maximal height is 79mm. Ideal height is 51mm.	
%
% All the coordinates are calculated for the orthogonal x/y system. 
% The orientation of the system is relative to center of gravity
% of the quadpod. All the shoulder servos are rotated. The angle 
% between the plane of servo and plane of quadpod's body is is 
% following the table: 
thetas=[45*pi/180,315*pi/180,225*pi/180,135*pi/180];

% XTable defines the gait. Technically is defines the shift between 
% the arms and the relative step size. As said, the shift defines 
% (in combination with Ztable) the gait.
Xtable=[
9/9, 6/9, 3/9, 0/9, 1/9, 2/9, 3/9, 4/9, 5/9, 6/9, 7/9, 8/9;
3/9, 4/9, 5/9, 6/9, 7/9, 8/9, 9/9, 6/9, 3/9, 0/9, 1/9, 2/9;
6/9, 7/9, 8/9, 9/9, 6/9, 3/9, 0/9, 1/9, 2/9, 3/9, 4/9, 5/9;
0/9, 1/9, 2/9, 3/9, 4/9, 5/9, 6/9, 7/9, 8/9, 9/9, 6/9, 3/9]

% Ztable defines the change of axis Z during the step. It
% defines the gain in combination with the Xtable
%Ztable=[
%0, 9, 9, 0,12,12, 0, 0, 0, 0, 0, 0;
%0, 0, 0, 0, 0, 0, 0, 9, 9, 0,12,12;
%0,12,12, 0, 9, 9, 0, 0, 0, 0, 0, 0;
%0, 0, 0, 0, 0, 0, 0,12,12, 0, 9, 9]

Ztable=[
-12, 9, 9,   9,12,12,   0, 0, 0,   0, 0, 0;
  0, 0, 0,   0, 0, 0, -12, 9, 9,   9,12,12;
  9,12,12, -12, 9, 9, -10, 0, 0,   0, 0, 0;
-10, 0, 0,   0, 0, 0,   9,12,12, -12, 9, 9]

% Array controls the quadrant for the x axis. The servo 1 and 4
% moves in 2nd, 3rd quadrat respectively, which corresponds with
% the negative end of axis x. Servos 2,3 moves in 4th and 1st 
% quadrant, which corresponds with the positive end of axis x.
signum=[-1,1,1,-1];

opt = menu( "Select type of model:", "step trajectory (2D)", "arm animation (3D)" );

step=input("Step length [0-98 mm]: ");

% Get the step direction. The step trajectory always intersects the 
% sweep point. This rotation takes as the center the sweet point
% and the step trajectory rotates around it.
% direction_angle=0;
direction_angle=input("Set angle to rotate the direction vector (0-180 deg): ");
drr=direction_angle*pi/180;
direction=[cos(drr),-sin(drr);sin(drr),cos(drr)];

x=0;
z=74;
z=input("Height [26-69 mm]: ");

servo_opt = menu( "Select servo type for the calculation:", "IQ-210", "TowerPro", "Ideal Servo" );

switch( servo_opt )
	case(1)
		servo_data = dlmread( "iq210.dat" );
	case(2)
		servo_data = dlmread( "towerpro.dat" );
	otherwise
		servo_data=[12000000,8;0.591,0.544]; % Ideal servo
endswitch

for arm=1:1:4
	figure();
	hold on;
	for k=1:1:12
		% define variable sh just for visualization (debugging)
		printf("Run %d - %d\n", arm, k );
		
	   % Define a sweet point for a arm. From within sweep point the arm can reach
		% the radius of half size of step.
		y = (step/2) - (Xtable(arm,k)*step);

		% Create the temporary vector for rotation. Direction of the step 
		T(1,1)=x;
		T(1,2)=y;
		disp("Original vector coordinates [x,y]:")
		disp(T)
		T=T*direction; % Rotate - find the direction of the movement.
		disp("Direction vector coordinates [x,y]:")
		disp(T)
		if( opt == 1 )
			plot(x,y,"4x");
			plot(T(1,1),T(1,2),"1*");
		endif

		% shift the rotated vector into the sweet point again
		% (coordinate x, the coordinate y is already determined 
		% from the Xtable). In this step we also determine the direction
		% by determining the sign for coordinate x.
		T(1,1)=T(1,1)+(signum(arm)*51);
		if( opt == 1 )
			plot(T(1,1),T(1,2),"5*");
		endif
		disp("Dislocated direction vector coordinates [x,y]:")
		disp(T)

		% Rotate the displaced vector for the servo mounting. 
		% The center of rotation is now the shoulder joint. 
		% Determine the z coordinate from Ztable.
		R=[cos(thetas(arm)),-sin(thetas(arm));sin(thetas(arm)),cos(thetas(arm))];
		P=T*R;
		disp("Rotated displaced vector coordinates [x,y]:");
		disp(P);
		RP(1,1)=P(1,1);
		RP(1,2)=P(1,2);
		RP(1,3)=z+Ztable(arm,k);
		disp("Final Servo vector coordinates [x,y,z]:");
		disp(RP);
		if( opt == 1 )
			plot(RP(1,1),RP(1,2),"2o");
			if( k == 1 ) 
				axis([-80,80,-80,80],"square");
				title('Step vetcor simulation')
				xlabel('x'); ylabel('y'); 
			endif
		endif

		% Convert P[x,y,z] -> alpha, beta, gama
		A=p2a([coxa,femur,tibia,base],RP);
		disp("Angles {gamma,alfa,beta}:");
		disp(A);
		printf( "OCR1A = {%d,%d,%d}\n", s2avr(A(2,1), servo_data, 1), s2avr(A(2,2), servo_data, 1), s2avr(A(2,3), servo_data, 0) );

		% kinematics terminates here ...
		% kinematics terminates here ...

		% ** TEST ** TEST ** TEST ** TEST **
		% ** TEST ** TEST ** TEST ** TEST **
		
		% Simulate the arm movement by creating 3D plot graphics ...
		if( opt == 2 )
			P1=a2p([coxa,femur,tibia,base],A);
			disp("3x [x,y,z] for all servo pivot points");
			disp(P1);
			
			a=linspace(0,P1(1,1),10);
			b=linspace(0,P1(1,2),10);
			c=P1(1,3).*ones(1,10);
			plot3(a,b,c);
			
			a=linspace(P1(1,1),P1(2,1),10);
			b=linspace(P1(1,2),P1(2,2),10);
			c=linspace(P1(1,3),P1(2,3),10);
			plot3(a,b,c,"g");
			
			a=linspace(P1(2,1),P1(3,1),10);
			b=linspace(P1(2,2),P1(3,2),10);
			c=linspace(P1(2,3),P1(3,3),10);
			plot3(a,b,c,"1");
	
			if( k == 1 ) 
				axis([-150,150,-150,150,0,150],"square");
				title('3DOF Simulation')
				xlabel('x'); ylabel('y'); zlabel('z') 
			endif
		endif

		drawnow;
	endfor
	if( opt == 2 )
		switch( arm )
		case(1)
			view( 315,30 );
		case(2)
			view( 45, 30 );
		case(3)
			view( 135, 30 );
		case(4)
			view( 225, 30 );
		otherwise
			disp( "Error rotating plot" );
		endswitch
	endif
	hold off;
endfor
